
using System.Collections.Generic;
using Battle;

public class PriorityQueue<T>
{
	protected List<int> priority = new List<int>();
	protected List<T> data = new List<T>();

	public T Dequeue()
	{
		if(Count<=0)
			return default(T);
		T min = data[0];
		Swap(0,Count-1);
		priority.RemoveAt(Count-1);
		data.RemoveAt(Count-1);
		if(Count>0)
			DownHeap(0);
		return min;
	}
	public void Enqueue(T d, int p)
	{
		int index = Count;
		data.Add(d);
		priority.Add(p);
		UpHeap(index);
	}
	
	public int Count
	{
		get { return data.Count; }
	}
	public void Clear()
	{
		priority.Clear();
		data.Clear();
	}

	protected void UpHeap(int start)
	{
		int i= start;
		int j= Parent(i);
		T tmp = data[i];
		while(j>=0)
		{
			if(Compare(j,i))
				break;
			Swap(i,j);
			i = j;
			j = Parent(i);
		}
		data[i] = tmp;
	}

	protected int Parent(int i)
	{
		if(i-1<0) return -1;
		return (i-1)/2;
	}

	protected void DownHeap(int start)
	{
		int i=start;
		T tmp = data[i];
		int j=i*2+1;
		while(j<Count)
		{
			if(j+1<Count && Compare(j+1,j))
				j++;
			if(Compare(i,j))
				break;
			Swap(i,j);
			i = j;
			j = 2*i +1;
		}
		data[i] = tmp;
	}

	protected bool Compare(int indexL, int indexR)
	{
		return priority[indexL]<priority[indexR];
	}

	protected void Swap(int indexL, int indexR)
	{
		int priTmp = priority[indexL];
		priority[indexL] = priority[indexR];
		priority[indexR] = priTmp;

		T tmp = data[indexL];
		data[indexL] = data[indexR];
		data[indexR] = tmp;
	}

}
public class AStar
{
	protected readonly Dictionary<Node,Node> cameFrom = new Dictionary<Node, Node>();
	protected readonly Dictionary<Node, int> costSoFar = new Dictionary<Node, int>(); 
	protected Map map;
	protected Node[] neighbours;
	protected PriorityQueue<Node> frontier;
	public AStar(Map m)
	{
		this.map = m;
		neighbours = new Node[Map.MaxNeighboursCount];
		frontier = new PriorityQueue<Node>();
	}
	void CalculateCameFrom(Node start,Node goal, Actor actor)
	{
		cameFrom.Clear();
		costSoFar.Clear();
		frontier.Clear();

		frontier.Enqueue(start,0);
		cameFrom[start] = start;
		costSoFar[start] = 0;
		while (frontier.Count > 0)
		{
			var current = frontier.Dequeue();
			
			//early exit
			if(current == goal)
				break;

			int neighboursCount = map.Neighbours(current,ref neighbours);
			for(int i=0;i<neighboursCount;++i)
			{
				Node next = neighbours[i];
				int newCost = costSoFar[current]+ map.Cost(current,next,actor);
				if(!cameFrom.ContainsKey(next) || newCost < costSoFar[next])
				{
					int priority = newCost + map.Heuristic(goal, next,actor);
					frontier.Enqueue(next, priority);
					
					costSoFar[next] = newCost;
					cameFrom[next] = current;
				}
			}
		}
	}
	
	public void GetPath(Node start, Node goal,Actor a,ref List<Node> path )
	{
		path.Clear();
		CalculateCameFrom(start, goal, a);
		var current = goal;
		path.Add(current);
		while (!Equals(current, start))
		{
			current = cameFrom[current];
			path.Add(current);
		}
		path.Reverse();
	}
}
